// --------------------------------------------------------------------------------------------------------------------
// <copyright file="StopOnContactMetricsCalculator.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation.Metrics
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Test.Robotics.Metrics;

    /// <summary>
    /// Agent that computes force/position performance metrics for "stop on contact" tasks.
    /// </summary>
    [DataContract]
    public class StopOnContactMetricsCalculator : ConsumerProducerAgent<ManipulatorStatusMessage, MetricsResultMessage>, IMessagePublisher<CompletionMessage>
    {
        /// <summary>
        /// Metrics result type for position result
        /// </summary>
        private const string positionResultsType = "StopOnContactPositionResult";

        /// <summary>
        /// Metrics result type for force result.
        /// </summary>
        private const string forceResultsType = "StopOnContactMaximumForceResult";

        /// <summary>
        /// Determines which axes to use to calcuate position.
        /// A 1 indicates that axis is used, a zero indicates it is not.
        /// </summary>
        [DataMember(Name = "PositionAxesMask")]
        private Vector3 positionAxesMask;

        /// <summary>
        /// Determines which axes to use to calcuate force magnitude.
        /// A 1 indicates that axis is used, a zero indicates it is not.
        /// </summary>
        [DataMember(Name = "ForceAxesMask")]
        private Vector3 forceAxesMask;

        /// <summary>
        /// The expected final resting position of the TCP if stop on contact was perfect.
        /// </summary>
        [DataMember(Name = "IdealPosition")]
        private Vector3 idealPosition;

        /// <summary>
        /// Robot ID string
        /// </summary>
        [DataMember(Name = "RobotIDString")]
        private string robotIDString;

        /// <summary>
        /// Duration after contact in ms to collect force in position data 
        /// </summary>
        [DataMember(Name = "DataCollectionDelayInMs")]
        private long dataCollectionDelayInMs;

        /// <summary>
        /// A task controller to activate/deactive (optional)
        /// </summary>
        [DataMember(Name = "TaskControllerName")]
        private string taskControllerName;

        /// <summary>
        /// The maximum magnitude of force observed during a run.
        /// </summary>
        private double maximumObservedForceMagnitude;

        /// <summary>
        /// The maximum positional error observed between contact and the end of run
        /// </summary>
        private double maximumObservedPositionError;

        /// <summary>
        /// Time at which contact was first observed;
        /// </summary>
        private double contactTime = -1;

        /// <summary>
        /// ID of the run
        /// </summary>
        private long runID;

        /// <summary>
        /// Inidicates if results have been emitted or not.
        /// </summary>
        private bool resultEmitted = false;

        /// <summary>
        /// Indicates if we have observed a non-contact condition during this trial
        /// Needed to ensure that we are not erronously interpreting feedback from
        /// the previous trial.
        /// </summary>
        private bool freeSpaceMotionObserved = false;

        /// <summary>
        /// Port to post completion messages to.
        /// </summary>
        private IForwarder<CompletionMessage> completionPort;

        /// <summary>
        /// Task controller that we will optionally activate/deactive
        /// </summary>
        private IAgentHost taskController;

        /// <summary>
        /// Initializes a new instance of the <c>StopOnContactMetricsCalculator</c> class.
        /// </summary>
        /// <param name="name">Agent name.</param>
        /// <param name="producer">ManipulatorStatusMessage producer</param>
        /// <param name="taskControllerName">Name of the task controller to active.</param>
        /// <param name="positionAxesMask">Mask determing which axes are used for position computation.</param>
        /// <param name="forceAxesMask">Mask determining which axes are used for force computation.</param>
        /// <param name="idealPosition">Idea stopping position.</param>
        /// <param name="robotIDString">The robot ID.</param>
        /// <param name="dataCollectionDelayInMs">Amount of time after detection of contact to continue collecting data.</param>
        public StopOnContactMetricsCalculator(
            string name,
            IProducer<ManipulatorStatusMessage> producer,
            string taskControllerName,
            Vector3 positionAxesMask,
            Vector3 forceAxesMask,
            Vector3 idealPosition,
            string robotIDString,
            long dataCollectionDelayInMs)
            : base(name, producer.Name)
        {
            if (!this.AllEntriesAreZeroOrOne(positionAxesMask))
            {
                throw new ArgumentException("All elements of positionAxesMask must be exactly 0 or 1");
            }

            if (!this.AllEntriesAreZeroOrOne(forceAxesMask))
            {
                throw new ArgumentException("All elements of forceAxesMask must be exactly 0 or 1");
            }

            if (idealPosition == null)
            {
                throw new ArgumentException("idealPosition cannot be null");
            }

            if (string.IsNullOrEmpty(robotIDString))
            {
                throw new ArgumentException("robotIDString cannot be null or empty");
            }

            this.positionAxesMask = positionAxesMask;
            this.forceAxesMask = forceAxesMask;
            this.idealPosition = idealPosition;
            this.robotIDString = robotIDString;
            this.maximumObservedForceMagnitude = 0;
            this.dataCollectionDelayInMs = dataCollectionDelayInMs;
            this.contactTime = -1;
            this.taskControllerName = taskControllerName;
        }

        /// <summary>
        /// Called when agent is activated.
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();

            this.freeSpaceMotionObserved = false;

            if (this.taskController != null)
            {
                this.taskController.Activate();
            }

            this.runID = AgentMessage.GetCurrentTime();
            
            this.maximumObservedForceMagnitude = 0;
            this.contactTime = -1;
            this.resultEmitted = false;
            this.maximumObservedForceMagnitude = 0;
            this.maximumObservedPositionError = 0;
        }

        /// <summary>
        /// Called when agent is deactivated
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();

            if (this.taskController != null)
            {
                this.taskController.Deactivate();
            }
        }

        /// <summary>
        /// Called to give us the port to use for publishing completion messages.
        /// </summary>
        /// <param name="forwarder">The completion port.</param>
        public void InitializePublisher(IForwarder<CompletionMessage> forwarder)
        {
            this.completionPort = forwarder;
        }

        /// <summary>
        /// Initialize the agent
        /// </summary>
        /// <param name="locator">Agent locator</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            if (!string.IsNullOrEmpty(this.taskControllerName))
            {
                this.taskController = locator.GetAgent(this.taskControllerName);
            }
            else
            {
                this.taskController = null;
            }
        }

        /// <summary>
        /// Handles <c>ManipulatorStatusMessage</c>
        /// </summary>
        /// <param name="message">A <c>ManpulationStatusMessage</c></param>
        public override void Receive(ManipulatorStatusMessage message)
        {
            if (this.resultEmitted)
            {
                return;
            }

            Vector3 tcpForce = new Vector3(
                this.forceAxesMask.X * message.State.TcpForceTorque[0],
                this.forceAxesMask.Y * message.State.TcpForceTorque[1],
                this.forceAxesMask.Z * message.State.TcpForceTorque[2]);

            // Measure maximum force observed throughout the trial.
            // Makes sure we capture the largest TCP force observed at any point
            // during motion.  Necessary for manually triggered KRL contacts to be
            // properly recorded and will capture cases where highest force magnitude
            // occurs prior to detection.
            if (tcpForce.Length() > this.maximumObservedForceMagnitude)
            {
                this.maximumObservedForceMagnitude = tcpForce.Length();
            }

            if (message.State.TcpUnexpectedForceOccured)
            {
                // If we have not observed a non-contact state, we are still getting feedback
                // from the last trial.
                if (!this.freeSpaceMotionObserved)
                {
                    return;
                }

                if (this.contactTime == -1)
                {
                    this.contactTime = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;
                }

                // compute an actual stopping position, substiuting ideal
                // positions for uncomputed axes.

                Vector3 actualStoppingPosition = message.State.TcpCartesianPosition.Position;

                if (this.positionAxesMask.X == 0)
                {
                    actualStoppingPosition.X = this.idealPosition.X;
                }

                if (this.positionAxesMask.Y == 0)
                {
                    actualStoppingPosition.Y = this.idealPosition.Y;
                }

                if (this.positionAxesMask.Z == 0)
                {
                    actualStoppingPosition.Z = this.idealPosition.Z;
                }

                if ((actualStoppingPosition - this.idealPosition).Length() > this.maximumObservedPositionError)
                {
                    this.maximumObservedPositionError = (actualStoppingPosition - this.idealPosition).Length();
                }

                if ((DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond) - this.contactTime >= this.dataCollectionDelayInMs)
                {
                    MetricsResultMessage positionMessage = new MetricsResultMessage(
                        positionResultsType,
                        0,
                        (actualStoppingPosition - this.idealPosition).Length(),
                        0,
                        1,
                        string.Empty,
                        0,
                        robotIDString,
                        runID,
                        message);

                    this.Publisher.Post(positionMessage);

                    MetricsResultMessage forceMessage = new MetricsResultMessage(
                        forceResultsType,
                        0,
                        maximumObservedForceMagnitude,
                        0,
                        1,
                        string.Empty,
                        0,
                        this.robotIDString,
                        this.runID,
                        message);

                    this.Publisher.Post(forceMessage);

                    Console.WriteLine(positionResultsType + ": " + (actualStoppingPosition - this.idealPosition).Length());
                    Console.WriteLine(forceResultsType + ": " + maximumObservedForceMagnitude);

                    this.completionPort.Post(new CompletionMessage(this.Name));

                    this.resultEmitted = true;
                }
            }
            else
            {
                this.freeSpaceMotionObserved = true;
            }
        } 

        /// <summary>
        /// Returns true if all entries of the vector are 0 or 1, false otherwise.
        /// </summary>
        /// <param name="vector">A <c>Vector3</c></param>
        /// <returns>True if all entries of the vector are 0 or 1</returns>
        private bool AllEntriesAreZeroOrOne(Vector3 vector)
        {
            if ((vector.X != 0 && vector.X != 1) ||
                (vector.Y != 0 && vector.Y != 1) ||
                (vector.Z != 0 && vector.Z != 1))
            {
                return false;
            }

            return true;
        }
    }
}
